#include "Visualizer.h"
#include <stdlib.h>

Visualizer::Visualizer(int width, int height):
out(height, width),
count(0)
	/*Writer("visualize.avi", CV_FOURCC('X','V','I','D'), 15, cv::Size(width, height))*/{
	this->width = width;
	this->height = height;
	colors = new unsigned char*[width * height];
	for(int i = 0; i < width * height; i++){
		colors[i] = new unsigned char[3];
		colors[i][0] = (unsigned char)rand();
		colors[i][1] = (unsigned char)rand();
		colors[i][2] = (unsigned char)rand();
	}
	Writer = cv::VideoWriter::VideoWriter("superpixel.avi", CV_FOURCC('X','V','I','D'), 3.0, cv::Size(width, height));
}
Visualizer::~Visualizer(){
}
void Visualizer::releaseVideo(){
	Writer.release();
}
void Visualizer::getRGB(float ratio, unsigned char* r, unsigned char* g, unsigned char* b){
	ratio = ratio >= 0.99 ? 0.99 : ratio;
	if(ratio < 0.33){
		*r = ratio/0.33 * 255;
	} else if(ratio < 0.66){
		*r = (0.66-ratio)/0.33 * 255;
		*g = (ratio-=0.33)/0.33 * 255;
	} else {
		*g = (0.99-ratio)/0.33 * 255;
		*b = (ratio-=0.66)/0.33 * 255;
	}
}

void Visualizer::setRGB(cv::Vec3b* ref, unsigned char r, unsigned char g, unsigned char b){
	ref->val[0] = r;
	ref->val[1] = g;
	ref->val[2] = b;
}

void Visualizer::display(int* in, int min, int max, int mode){
	unsigned t1 = clock();

	int* look;
	if(mode == device){
		look = new int[width * height];
		cudaMemcpy(look, in, width * height * sizeof(int), cudaMemcpyDeviceToHost);
		in = look;
	}


	unsigned char r = 0, g = 0, b = 0;

	unsigned char* c_ref;
	cv::Vec3b* ref;
	for(int i = 0; i < height; i++){
		for(int j = 0; j < width; j++){
			if(in[i * width + j] > -1){
				c_ref = colors[in[i * width + j]];
				out.at<cv::Vec3b>(cv::Point2d(j, i)).val[0] = c_ref[0];
				out.at<cv::Vec3b>(cv::Point2d(j, i)).val[1] = c_ref[1];
				out.at<cv::Vec3b>(cv::Point2d(j, i)).val[2] = c_ref[2];
			} else {
				out.at<cv::Vec3b>(cv::Point2d(j, i)).val[0] = 0;
				out.at<cv::Vec3b>(cv::Point2d(j, i)).val[1] = 0;
				out.at<cv::Vec3b>(cv::Point2d(j, i)).val[2] = 0;
			}
		}
	}

	cv::cvtColor(out, out, CV_BGR2RGB);
	cv::imshow("segment", out);
	cv::waitKey(1);


	//delete[] colors;

	if(mode == device)
		delete[] look;


	std::cout << clock() - t1 << "for random visualization" << std::endl;

}


void Visualizer::display(short* in, int min, int max, int mode){
	short* look;	
	if(mode == device){
		look = new short[width * height];
		cudaMemcpy(look, in, width * height * sizeof(short), cudaMemcpyDeviceToHost);
		in = look;
	}

	unsigned char r = 0, g = 0, b = 0;
	cv::Vec3b* ref;
	for(int i = 0; i < height; i++){
		for(int j = 0; j < width; j++){
			getRGB((1.0 * in[i * width + j]) / max, &r, &g, &b);
			setRGB(&out.at<cv::Vec3b>(cv::Point2d(j, i)), r, g, b);
		}
	}

	cv::cvtColor(out, out, CV_BGR2RGB);
	cv::imshow("depth_raw", out);
	cv::waitKey(1);

	if(mode == device)
		delete[] look;
}

void Visualizer::display(float* in, float min, float max, int mode){
	float* look;
	if(mode == device){
		look = new float[width * height];
		cudaMemcpy(look, in, width * height * sizeof(float), cudaMemcpyDeviceToHost);
		in = look;
	}

	
	for(int i = 0; i < height; i++){
		for(int j = 0; j < width; j++){
			unsigned char r = 0, g = 0, b = 0;
			float d = 1.0 * in[i * width + j] / max;
			if(d > 0.0)
				getRGB(d, &r, &g, &b);
			setRGB(&out.at<cv::Vec3b>(cv::Point2d(j, i)), r, g, b);
		}
	}
	
	cv::cvtColor(out, out, CV_BGR2RGB);
	cv::imshow("depth_filtered", out);
	cv::waitKey(1);
	
	if(mode == device)
		delete[] look;
}

void Visualizer::display(float3* in, int mode){
	float3* look;
	if(mode == device){
		look = new float3[width * height];
		cudaMemcpy(look, in, width * height * sizeof(float3), cudaMemcpyDeviceToHost);
		in = look;
	}


	for(int i = 0; i < height; i++)
		for(int j = 0; j < width; j++)
			setRGB(
				&out.at<cv::Vec3b>(cv::Point2d(j, i)), 
				abs(in[i * width + j].x * 255.0), 
				abs(in[i * width + j].y * 255.0), 
				abs(in[i * width + j].z * 255.0)
			);

	cv::cvtColor(out, out, CV_BGR2RGB);
	cv::imshow("normal", out);
	cv::waitKey(1);

	if(mode == device)
		delete[] look;

}

void Visualizer::display(xn::ImageMetaData* img){
	memcpy(out.data, img->Data(), out.cols * out.rows * out.elemSize());
	cv::cvtColor(out, out, CV_BGR2RGB);
	cv::imshow("rgb", out);
	cv::waitKey(1);
}


////////////////////////////////////////////////////////////////////////////////////segment
void Visualizer::display(const cv::Mat in, const cv::Mat_<int> label,  const std::string& name){
	memcpy(out.data, in.data, out.cols * out.rows * out.elemSize());
	//cv::cvtColor(out, out, CV_BGR2RGB);
	//std::cout <<  label->rows <<std::endl;
	for(int i = 1; i < label.rows -1; i++){
		for(int j = 1; j < label.cols -1
			; j++){
			for(int ii = i - 1; ii <= i + 1; ii++){
				for(int jj = j - 1; jj <= j + 1; jj++){
					if(label.at<int>(i, j) != label.at<int>(ii, jj)){
						out.at<cv::Vec3b>(i, j) = cv::Vec3b(0, 0, 0);
					}
				}
			}
		}
	}
	/*std::ostringstream image_name;
	image_name << "./result/segmented_" << id;
	*/
	Writer << out;
	cv::imshow(name, out);
	cv::waitKey(1);
	//std::ostringstream oss;
	//oss << name << count << ".bmp";
	//char key = cv::waitKey(1);
	//if( key == 's' ){
	//	cv::imwrite(oss.str(), out);
	//	std::cout << "save" <<std::endl;
	//	count++;
	//}
	//cv::imwrite(name, out);
}
///////////////////////////////////////////////////////////////////////////////////////color
void Visualizer::display(cv::Mat& in, const std::string& name){
	memcpy(out.data, in.data, out.cols * out.rows * out.elemSize());
	
	cv::imshow(name, out);
	cv::waitKey(0);
	cv::imwrite(name, out);
}


void Visualizer::display(xn::ImageMetaData* img, cv::Mat_<int>* label){
	memcpy(out.data, img->Data(), out.cols * out.rows * out.elemSize());
	cv::cvtColor(out, out, CV_BGR2RGB);
	for(int i = 1; i < label->rows -1; i++){
		for(int j = 1; j < label->cols -1
			; j++){
			for(int ii = i - 1; ii <= i + 1; ii++){
				for(int jj = j - 1; jj <= j + 1; jj++){
					if(label->at<int>(i, j) != label->at<int>(ii, jj)){
						out.at<cv::Vec3b>(i, j) = cv::Vec3b(0, 0, 0);
					}
				}
			}
		}
	}
	cv::imshow("rgb", out);
	cv::waitKey(1);
	
}



void Visualizer::display(xn::DepthMetaData* depth){
	
	for(int i = 0; i < height; i++){
		for(int j = 0; j < width; j++){
			unsigned char r = 0, g = 0, b = 0;
			float d = (*depth)(j, i) / 3000.0 ;
			std::cout << d << std::endl;
			if(d > 0.0)
				getRGB(d, &r, &g, &b);
			setRGB(&out.at<cv::Vec3b>(cv::Point2d(j, i)), r, g, b);
		}
	}

	cv::cvtColor(out, out, CV_BGR2RGB);
	cv::imshow("depth_original", out);	
	cv::waitKey(1);
}
///////////////////////////////////////////////////////////////////////////////////////////////////depth
void Visualizer::display(cv::Mat_<double>* depth, const std::string& name){
	
	//std::cout << out.cols << "\t" << out.rows << std::endl;
	
	for(int i = 0; i < depth->rows; i++){
		for(int j = 0; j < depth->cols; j++){
			out.at<cv::Vec3b>(cv::Point2d(j, i)).val[0] = 0;
			out.at<cv::Vec3b>(cv::Point2d(j, i)).val[1] = 0;
			out.at<cv::Vec3b>(cv::Point2d(j, i)).val[2] = 0;
			unsigned char r = 0, g = 0, b = 0;
			float d = (float)(depth->at<double>(i, j) / 5500.0);
			//std::cout << d <<std::endl;
			if(d > 0.0){
				getRGB(d, &r, &g, &b);
				//std::cout << j << "\t" << i << "\t" << d << std::endl;
				setRGB(&out.at<cv::Vec3b>(cv::Point2d(j, i)), r, g, b);
			}
		}
	}

	cv::cvtColor(out, out, CV_BGR2RGB);
	
	cv::imshow(name, out);	
	
	//cv::imwrite(name, out);
}
void Visualizer::fileOut(int* in, const char* str, int mode){
	int* look;
	if(mode == device){
		look = new int[width * height];
		cudaMemcpy(look, in, width * height * sizeof(int), cudaMemcpyDeviceToHost);
		in = look;
	}

	std::fstream f(str, std::ios_base::out);
	for(int y = 0; y < height; y++){
		for(int x = 0; x < width; x++){
			f << in[x + y * width] << "\t";
		}
		f << std::endl;
	}

	f.close();

	std::cout << "file out finished" << std::endl;

	if(mode == device)
		delete[] look;

}

void Visualizer::fileOut(float* in, const char* str, int mode){
	float* look;
	if(mode == device){
		look = new float[width * height];
		cudaMemcpy(look, in, width * height * sizeof(float), cudaMemcpyDeviceToHost);
		in = look;
	}

	std::fstream f(str, std::ios_base::out);
	for(int y = 0; y < height; y++){
		for(int x = 0; x < width; x++){
			f << in[x + y * width] << "\t";
		}
		f << std::endl;
	}

	f.close();

	std::cout << "file out finished" << std::endl;

	if(mode == device)
		delete[] look;
}

void Visualizer::fileOut(bool* in, const char* str, int mode){
	bool* look;
	if(mode == device){
		look = new bool[width * height];
		cudaMemcpy(look, in, width * height * sizeof(bool), cudaMemcpyDeviceToHost);
		in = look;
	}

	std::fstream f(str, std::ios_base::out);
	for(int y = 0; y < height; y++){
		for(int x = 0; x < width; x++){
			if(in[x + y * width])
				f << "1" << "\t";
			else
				f << "0" << "\t";
		}
		f << std::endl;
	}

	f.close();

	std::cout << "file out finished" << std::endl;

	if(mode == device)
		delete[] look;



}
